#!/usr/bin/env python
# -*- coding: utf-8 -*-

import cv2
import time
import envs
import rospy

from img_utils import ros_to_cv2

from sensor_msgs.msg import Image


class ImgTools():
    def __init__(self):
        # 图像订阅者
        self.image_sub = rospy.Subscriber('/camera/image_raw', Image, callback=self.image_callback)

        # 图像数据
        self.img_data = None

        # 图像序列号
        self.img_id = 0

        time.sleep(0.2)
        rospy.spin()

    def image_callback(self, data):
        rospy.logdebug('image callback...')

        self.img_data = data

    def img_save(self):
        """
        保存一张图像
        """
        rospy.loginfo('img save...')

        if self.img_data:
            img = ros_to_cv2(self.img_data, encoding='bgr8')
            filename = '../imgs/{}.jpg'.format(self.img_id)
            cv2.imwrite(filename, img)
            self.img_id += 1

            rospy.loginfo('Image has been saved to %s', filename)

        rospy.loginfo('img save OK')


if __name__ == '__main__':
    rospy.init_node('npu_robot_img_tools', anonymous=True, log_level=envs.log_level)
    img_tools = ImgTools()
    img_tools.img_save()
